#include <iostream>

using namespace std;

//FAKE FUNCTIES ZODAT HET WERKT IN C++
const int INPUT = 0;
int pinMode(int x, int y)
{
    return 0;
}
int analogRead(int pin);
unsigned int fakeAnalogSensors[5] = {100,500,1000,1000,1000}; // zogezegde start instellingen van de sensoren
void setFakeAnalogSensors(unsigned int fakesensorz[5]);
void setBFakeAnalogSensors(bool fakesensorz[5]);

void debug(bool a[5]);



/* &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&BEGIN ARDUINO&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/


//=================================================================================
//=============================== ALGEMENE SETUP ==================================
bool aArg[5] = {0,0,0,0,0};
bool *makeBArray(bool a, bool b, bool c, bool d, bool e)
{
    aArg[0] = a;
    aArg[1] = b;
    aArg[2] = c;
    aArg[3] = d;
    aArg[4] = e;
    return &aArg[0];
}


//================================= PID CONSTANTS =================================
const float PIDsettings[2][3] = { {4.5, 0.8, 1.5}, {2.5,0.95, 4.105} };
const char PIDsetting = 1;
const float kP = PIDsettings[PIDsetting][0];    //proportioneel: is gewoon hoe fout hij zit. Net zoals de 2 sensor robot van school.
const float kI = PIDsettings[PIDsetting][1];     //integraal: wordt groter naargelang de tijd
const float kD = PIDsettings[PIDsetting][2];     //afgeleide: vergelijkt verschil foutmarge
// andere robot gebruikte: 0.05 0.000001 1.105
//=================================================================================
//================================= PID VARIABELEN ================================
float hP; // P is nooit groot komma getal
double hI, hD;
int vorigeFoutMarge = 0;
int foutMarge = 0;
double PID = 0;
signed char mPos = 0;
//=================================   SETTINGS  ===================================
const unsigned char analogInputSensors[5] = {1, 5, 6, 7, 9}; // de pins op arduino

const unsigned char memoryPID = 20; // aantal stappen die hij moet onthouden
const unsigned char memorySensors = 10; // ^^

bool curSensors[5]={0,0,0,0,0}, prevSensors[memorySensors][5];
float prevPID[memoryPID];
//======================== lichtsterkte calibratie ========================
const bool lichtCalibratie = false; // aan of af zetten
const int maxLichtVerschil = 100; // belangrijk!!!!!!!!!!!
const int aantalMetingen = 100; // belangrijk!!!!!!!!!!!
unsigned int lichtMarge = 900; // als lichtCalibratie af staat gebruiken we dit!
//=========================================================================
void insertionSort(int x[],int length) // gebruikt bij lichtcalibratie om waarden te sorteren
{
  int key,i;
  for(int j=1;j<length;j++)
  {
     key=x[j];
     i=j-1;
     while(x[i]>key && i>=0)
     {
               x[i+1]=x[i];
         i--;
     }
     x[i+1]=key;
  }
}
unsigned int bepaalLichtMarge(int waarden[])
{
    insertionSort(waarden, aantalMetingen);
    int n = 0; // dit zal het element zijn waarbij we splitsen tussen min en max
    for(n; n < aantalMetingen; n++)
        if( (n!=0) && ( (waarden[n] - waarden[n-1]) > maxLichtVerschil ) )
            break;

    unsigned int somMax = 0;
    unsigned int somMin = 0;
    for(int x = 0; x < n; x++)
        somMin += waarden[x];

    for(int x = n; x < (aantalMetingen); x++)
        somMax += waarden[x];
    //somMax/(aantalMetingen-n) is het gemiddelde van alle max waarden en somMin/(n) gemiddelde van alle min waarden
    return ( (somMax/(aantalMetingen-n)) + (somMin/(n)) )/2;
}
void leesAnalogSensors()
{


    // lees sensors en vergelijk met lichtmarge
    for(int i=0; i<5; i++)
    {
        curSensors[i] = (analogRead(analogInputSensors[i]) > lichtMarge) ? 1 : 0;
    }


    //onthoud de sensor waarden zodat we die later misschien kunnen gebruiken om parcour te herkennen

    // beweeg prevSensors 1 om plaats te maken voor huidige sensor
    for(int x = (memorySensors-1); x >0; x--)
        for(int i = 0; i<5; i++)
            prevSensors[x][i] = prevSensors[x-1][i];
     // zet eerste prevSensors gelijk aan huide sensors
     for(int i = 0; i <5; i++)
        prevSensors[0][i] = curSensors[i];

}
bool compareSensors2(bool a[5], bool b[5])
{
	for(int i = 0; i < 5; i++)
		if(a[i] != b[i])
			return false;
	return true;
}
bool compareSensors(bool a[5])
{
    return compareSensors2(a, curSensors);
}
void calcmPos() // bepaling van de foutmarge en later uitzonderingen herkennen(kan pas vanaf we robot hebben)
{
    if(compareSensors(makeBArray(0,0,0,0,1)))
        mPos = 4;
    else if(compareSensors(makeBArray(0,0,0,1,1)))
        mPos = 3;
    else if(compareSensors(makeBArray(0,0,0,1,0)))
        mPos = 2;
    else if(compareSensors(makeBArray(0,0,1,1,1)))
        mPos = 1;
    else if(compareSensors(makeBArray(0,0,1,1,0)))
        mPos = 1;
    else if(compareSensors(makeBArray(0,0,1,0,0)))
        mPos = 0;
    else if(compareSensors(makeBArray(0,1,1,0,0)))
        mPos = -1;
    else if(compareSensors(makeBArray(1,1,1,0,0)))
        mPos = -1;
    else if(compareSensors(makeBArray(0,1,0,0,0)))
        mPos = -2;
    else if(compareSensors(makeBArray(1,1,0,0,0)))
        mPos = -3;
    else if(compareSensors(makeBArray(1,0,0,0,0)))
        mPos = -4;
    else if(compareSensors(makeBArray(1,1,1,1,1)) && (prevSensors[0][0] == 0 && prevSensors[0][4] == 0)) // kruispunt???
        mPos = 0;
    else if(compareSensors(makeBArray(0,0,0,0,0))) // weg kwijt <-> onderbroken ?????
        mPos = ((mPos<0) ? -5 : 5); // WEG KWIJT ZOEK WEG OPNIEUW
    else if( compareSensors(makeBArray(1,0,1,0,0)) || compareSensors(makeBArray(0,0,1,0,1)) )
        mPos = 0; // rechtdoor gaan als er bv buitenste lijn gezien wordt maar daartussen niet
    else if( compareSensors(makeBArray(1,0,1,0,1)))
        mPos = 0; // gewoon een foutmarge -> rechtdoor
}
void resetHeuristics() // alle waarden van algoritme op 0 zetten
{
    foutMarge = 0;
    mPos = 0;
    hP = 0;
    hI = 0;
    hD = 0;
    PID = 0;
}
void calcHeuristics() // verschillende waarden van het algoritme berekenen
{
    if(mPos != 0)
    {
        foutMarge = mPos;
        hP = kP * mPos;
        hI = (hP + hI) * kI;
        hD = (foutMarge - vorigeFoutMarge) * kD;

        vorigeFoutMarge = foutMarge;
        PID = hP + hI + hD;
    }
    else
    {
        resetHeuristics();
    }
    // beweeg prevPID 1 om plaats te maken voor huidige PID, onthoud onze PID waarden zodat we dit ook kunnen gebruiken om parcour te herkennen
    for(int x = (memoryPID-1); x >0; x--)
        prevPID[x] = prevPID[x-1];
     // zet laatste prevPID gelijk aan huide PID
    prevPID[0] = PID;
}

void setup()
{
    // hier moet nog code komen om metingen te doen voor lichtmarge, pas ook aantalMetingen aan!!!
    //if(lichtCalibratie) lichtMarge = bepaalLichtMarge(metingen, aantalMetingen); // lichtcalibratie enzo
    for(int i=0; i<5; i++)
        pinMode(analogInputSensors[i], INPUT); // pinnen herkennen als inputs

    //vul prevSensors met 0 --> initialiseer ze
    for(int n = 0; n < memorySensors; n++)
        for(int i = 0; n < 5; n++)
            prevSensors[n][i] = 0;

    //vul prevPID met 0 --> initialiseer ze
    for(int n = 0; n < memoryPID; n++)
            prevPID[n] = 0;
}
void loop()
{
    leesAnalogSensors();
    calcmPos();
    calcHeuristics();
}
/* &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&  EINDE ARDUINO   &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&
&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&*/
int main()
{

    setup(); // dit sowieso eerst!!!!
    // debug is hetzelfde als loop() behalve dat het ook gegevens print en dat je de huidige sensor waarden kunt faken

    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,1,0));
    debug(makeBArray(0,0,1,1,0));
    debug(makeBArray(0,0,1,1,1));
    debug(makeBArray(0,0,1,1,1));
    debug(makeBArray(0,0,0,1,1));
    debug(makeBArray(0,0,0,1,1));
    debug(makeBArray(0,0,0,1,1));
    debug(makeBArray(0,0,0,1,1));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));
    debug(makeBArray(0,0,1,0,0));

    float testPID = 0;
    for(int i=0; i<memoryPID; i++)
        testPID += prevPID[i];
    int marge = 100;
    cout << endl;
    cout << "laatste 20 PID afwijkingen: " << testPID << endl;
    if(testPID < marge && testPID > (-marge))
        cout << "Heeft een rechtlijnig stuk afgelegd!";
    else if(testPID > marge)
        cout << "Heeft net een kromme bocht naar links gemaakt";
    else
        cout << "Heeft net een kromme bocht naar rechts gemaakt";
    cout << "\n\n\n";
    int lichtWaarden[] = {145,127,116,178,203,240,245,144,236,192,272,235,147,202,214,107,210,177,130,144,227,135,144,132,143,158,200,202,142,214,108,114,223,128,165,283,119,245,158,241,189,238,186,116,300,223,143,172,255,235,664,606,605,766,758,649,856,850,724,626,751,784,860,851,727,756,779,839,738,800,718,858,744,797,757,644,879,793,643,869,745,614,841,692,671,718,860,810,755,671,896,738,838,821,821,674,639,795,833,672};
    cout << "Test lichtmarge:  verwachting [ 469 ] uitkomst [ " << bepaalLichtMarge(lichtWaarden) << " ]" << endl;
    return 0;
}


int analogRead(int pin)
{
    for(int i = 0; i<5; i++)
        if(analogInputSensors[i] == pin)
            return fakeAnalogSensors[i];
}
void setFakeAnalogSensors(unsigned int sensorz[5])
{
    for (int i = 0; i<5; i++)
        fakeAnalogSensors[i] = sensorz[i];
}
void setBFakeAnalogSensors(bool sensorz[5])
{
    for (int i = 0; i<5; i++)
        fakeAnalogSensors[i] = sensorz[i]*1023;
}
void debug(bool a[5])
{
    setBFakeAnalogSensors(a);
    loop();
    cout << "  ";
    for(int i = 0; i<5; i++)
        cout << curSensors[i] << " ";
    cout << "\t" << ( (PID >= 0) ? " " : "" ) << PID;
    cout << "      \t" << "( " <<  (hP >=0 ? " " : "") << hP << "\t" << "\t" <<  (hI >=0 ? " " : "") << hI << "       \t" <<  (hD >=0 ? " " : "") << hD << " )" << endl;
}

//====================================
